//////////////
// IF Range Swiches
int setup_rangeswiches(){
  pinMode(switch_body_back_left, INPUT);
  pinMode(switch_body_back_center, INPUT);  
  pinMode(switch_body_back_right, INPUT);
}

boolean get_hit_switch(const int iInfraSwitchPin)
{
  return (digitalRead(iInfraSwitchPin)==HIGH)?true:false;
}

//////////////
// Sharp gp2ya02 IR Range Sensors (20-150cm)
short get_range_sensor_gp2ya02(const int iInfraPin, const int iCount, const int iDelay, const float fBeamPrecision, const float fMin, const float fMax)
{
  float fValMin = -1, fValMax = -1;
  float fDistance = 0;
  float fCount = 0;
  boolean bLimitRange = (fMax>fMin)?true:false;
  boolean bUsePrecision = (fBeamPrecision>0)?true:false;

  for(int i=0; i<iCount; i++)
  {
    if(iDelay>0) delay(iDelay);

    const float fScale = 5.0f/1024.0f;    // value from sensor * (5/1024) - if running 3.3.volts then change 5 to 3.3
    int iAnalogValue = analogRead(iInfraPin);
    float fVolts = iAnalogValue*fScale;

    if(fVolts>0.4 && fVolts<2.5)
    {
      // worked out from graph 65 = theretical distance / (1/Volts)S - luckylarry.co.uk
      float fVal = 65.0f*pow(fVolts, -1.10f); 
      if(!bLimitRange || (bLimitRange && fVal>=fMin && fVal<=fMax))
      {
        if(bUsePrecision)
        {
          if(fValMax==-1 || fVal>fValMax) fValMax = fVal;      
          else if(fValMin==-1 || fVal<fValMin) fValMin = fVal;
        }
        fDistance += fVal;
        fCount ++;
      }
    }
  }

  // Sum result
  if(fCount>1)
  {
    fDistance /= fCount;

    // ecart trop important = bad result
    if(bUsePrecision && abs(fValMax)-abs(fValMin)>((fMax-fMin)*fBeamPrecision)) return bLimitRange?fMax:-1;
  }
  else if(fCount==0)
    return -1;

  if(bLimitRange)
  {
    // out of range ?
    if(fDistance>fMax) fDistance = fMax;
    else if(fDistance<fMin) fDistance = fMin;
  }

  // retour en cm
  return (short)fDistance;
}

//////////////
// Sharp gp2ya21 IR Range Sensors (10-80cm)
short get_range_sensor_gp2ya21(const int iInfraPin, const int iCount, const int iDelay, const float fBeamPrecision, const float fMin, const float fMax)
{
  float fValMin = -1, fValMax = -1;
  float fDistance = 0;
  float fCount = 0;
  boolean bLimitRange = (fMax>fMin)?true:false;
  boolean bUsePrecision = (fBeamPrecision>0)?true:false;

  for(int i=0; i<iCount; i++)
  {
    if(iDelay>0) delay(iDelay);

    const float fScale = 5.0f/1024.0f;    // value from sensor * (5/1024) - if running 3.3.volts then change 5 to 3.3
    int iAnalogValue = analogRead(iInfraPin);
    float fVolts = iAnalogValue*fScale;

    // worked out from graph 27 = theretical distance / (1/Volts)S - luckylarry.co.uk
    if(fVolts>0.4 && fVolts<2.25)
    {
      float fVal = 32.0f*pow(fVolts, -1.10f);
      if(!bLimitRange || (bLimitRange && fVal>=fMin && fVal<=fMax))
      {
        if(bUsePrecision)
        {
          if(fValMax==-1 || fVal>fValMax) fValMax = fVal;      
          else if(fValMin==-1 || fVal<fValMin) fValMin = fVal;
        }
        fDistance += fVal;
        fCount ++;
      }
    }
  }

  // Sum result
  if(fCount>1)
  {
    fDistance /= fCount;

    // ecart trop important = bad result
    if(bUsePrecision && abs(fValMax)-abs(fValMin)>((fMax-fMin)*fBeamPrecision))
      return bLimitRange?fMax:-1;
  }

  if(bLimitRange)
  {
    // out of range ?
    if(fCount==0 || fDistance>fMax) fDistance = fMax;
    else if(fDistance<fMin) fDistance = fMin;
  }

  // retour en cm
  return (short)fDistance;
}

//////////////
// Ultra Sonic Sensors (3-600cm)
short get_range_sensor_ultrasonic(const int iSonicPin, const int iCount, const int iDelay, const float fBeamPrecision, const float fMin, const float fMax)
{
  float fValMin = -1, fValMax = -1;
  float fDistance = 0;
  float fCount = 0;
  boolean bLimitRange = (fMax>fMin)?true:false;
  boolean bUsePrecision = (fBeamPrecision>0)?true:false;

  for(int i=0; i<iCount; i++)
  {
    if(iDelay>0) delay(iDelay);

    pinMode(iSonicPin, OUTPUT);
    digitalWrite(iSonicPin, LOW);
    delayMicroseconds(2);
    digitalWrite(iSonicPin, HIGH);
    delayMicroseconds(5);
    digitalWrite(iSonicPin, LOW);

    pinMode(iSonicPin, INPUT);
    long duration = pulseIn(iSonicPin, HIGH);

    float fVal = (float)duration/57.0f;
    if(!bLimitRange || (bLimitRange && fVal>=fMin && fVal<=fMax))
    {
      if(bUsePrecision)
      {
        if(fValMax==-1 || fVal>fValMax) fValMax = fVal;      
        else if(fValMin==-1 || fVal<fValMin) fValMin = fVal;
      }
      fDistance += fVal;
      fCount ++;
    }
  }

  // Sum result
  if(fCount>1)
  {
    fDistance /= fCount;

    // ecart trop important = bad result
    if(bUsePrecision && abs(fValMax)-abs(fValMin)>((fMax-fMin)*fBeamPrecision)) return bLimitRange?fMax:-1;
  }
  else if(fCount==0)
    return -1;

  if(bLimitRange)
  {
    // out of range ?
    if(fDistance>fMax) fDistance = fMax;
    else if(fDistance<fMin) fDistance = fMin;
  }

  // retour en cm
  return (short)fDistance;
}

